
#include "PhotoSwitch.h"

#define defaultSteeringGain 0.3
#define defaultLeftSpeed -0.5
#define defaultRightSpeed -0.5
#define lineStyle 0 //1 for Dark, 0 for Light

PhotoSwitch::PhotoSwitch(Drive *y)
{
	p1 = new DigitalInput(1);//dark 1
	p2 = new DigitalInput(2);//dark 2
	p3 = new DigitalInput(3);//dark 3
	p4 = new DigitalInput(4);//light 1
	p5 = new DigitalInput(5);//light 2
	p6 = new DigitalInput(6);//light 3
	sensor = new Sensor(y);
	binary = 0;
	steeringGain = 0;
	leftSpeed = 0;
	rightSpeed = 0;
	//s1 = p1->Get();
	//s2 = p2->Get();
	//s3 = p3->Get();
	//printf("Left Dark: %f\n" , p1->Get());
	//printf("Center Dark: %f\n" , p2->Get());
	//printf("Right Dark: %f\n" , p3->Get());
	//printf("Left Light: %f\n" , p4->Get());
	//printf("Center Light: %f\n" , p5->Get());
	//printf("Right Light: %f\n" , p6->Get());
		
}

float PhotoSwitch::drive(int x)//parameter of fork selection: 0 for left, 1 for right
{
	if(lineStyle == 1){
		binary = ((onDark()) + (onDark3()*3) + (dRight()*2) + (onDark1() *2) + (dLeft()*3) + ((allDark()*4) ));
		if (binary == 4){
			binary += x;
		}//end of embedded if
	}else if (lineStyle == 0){
		binary = onLight() + (lRight()*2) + (lLeft()*3) + ((allLight()*4) + x);
	}else{
		binary = 0;
	}//end of else
	
	switch(binary){
	case 1: //on line
		printf("Case 1\n");
		leftSpeed = defaultLeftSpeed;
		rightSpeed = defaultRightSpeed;
		//printf("leftSpeed: %f\n", leftSpeed);
		//printf("rightSpeed: %f\n", rightSpeed);
		//drive straight
		break;
	case 2: //error to the left
		printf("Case 2\n");
		leftSpeed = 0.0;
		rightSpeed = -0.7;//correct to the right
		//printf("Right Speed: %i \n", rightSpeed);
		break;
	case 3: //error to the right
		printf("Case 3\n");
		leftSpeed = -0.7;
		rightSpeed = 0.0;//correct to the left
		//printf("Left Speed: %i \n", leftSpeed);
		break;
	case 4: //ALL Dark; fork to the left
		printf("Case 4\n");
		leftSpeed = 0.0;
		rightSpeed = 0.0;
		//sensor->gyroSubroutine(0);
		break;
	case 5: //ALL Dark; fork to the right
		printf("Case 5\n");
		leftSpeed = 0.0;
		rightSpeed = 0.0;
		//sensor->gyroSubroutine(1);
		break;
	case 6:
	case 0:
		printf("Case 0\n");
		//no sensors - do nothing
		leftSpeed = -0.2;
		rightSpeed = -0.2;
		break;
	default: //what is wrong??
		printf("Default\n");
		leftSpeed = 0;
		rightSpeed = 0;
		//do nothing
		break;
	}//end of binary switch
	return steeringGain;
}//end of drive

float PhotoSwitch::getLeftSpeed()
{
	return leftSpeed;
}
float PhotoSwitch::getRightSpeed()
{
	return rightSpeed;
}

int PhotoSwitch::onDark()//following Dark line accurately
{
	if((p2->Get()== 0) && (p1->Get() == 1) && (p3->Get() == 1))
	{ return 1; } else { return 0; }
}//end of onDark
int PhotoSwitch::onDark1()//follow Dark line
{
	if((p1->Get() == 0) && (p2->Get() == 1) && (p3->Get() == 1))
	{ return 1; } else { return 0; }	
}
int PhotoSwitch::onDark3()//follow Dark line
{
	if((p1->Get() == 1) && (p2->Get() == 1) && (p3->Get() == 0))
	{ return 1; } else { return 0; }	
}
int PhotoSwitch::dLeft()//error to the left
{
	if((p2->Get()== 0) && (p1->Get() == 1) && (p3->Get() == 0))
	{ return 1; } else { return 0; }
}//end of dLeft
int PhotoSwitch::dRight()//error to the right
{
	if((p2->Get()== 0) && (p1->Get() == 0) && (p3->Get() == 1))
	{ return 1; } else { return 0; }
}//end of dRgiht
int PhotoSwitch::onLight()//following Light line accurately
{
	if((p5->Get()== 0) && (p4->Get() == 1) && (p6->Get() == 1))
	{ return 1; } else { return 0; }
}//end of onLight
int PhotoSwitch::lLeft()//error to the left
{
	if((p5->Get()== 0) && (p4->Get() == 1) && (p6->Get() == 0))
	{ return 1; } else { return 0; }
}//end of lLeft
int PhotoSwitch::lRight()//error to the right
{
	if((p5->Get()== 0) && (p4->Get() == 0) && (p6->Get() == 1))
	{ return 1; } else { return 0; }
}//end of lRight
int PhotoSwitch::allDark()//Dark fork
{
	if((p2->Get()== 0) && (p1->Get() == 0) && (p3->Get() == 0))
	{ return 1; } else { return 0; }
}//end of allDark
int PhotoSwitch::allLight()//Light fork
{
	if((p5->Get()== 0) && (p4->Get() == 0) && (p6->Get() == 0))
	{ return 1; } else { return 0; }
}//end of allLight

PhotoSwitch::~PhotoSwitch()
{
	printf("*****Photoswitch Deconstructor Called*****");
	delete sensor;
	sensor = NULL;
}//end of deconstructor





